typedef struct SPid
{
  double dState;      	// Last position input
  double dError;		// Last error value
  double iState;      	// Integrator state
  double iMax, iMin;  	
  // Maximum and minimum allowable integrator state
  double	iGain,    	// integral gain
        	pGain,    	// proportional gain
         	dGain;     	// derivative gain
  double	Position;	// Desired Position, used to hold value for its own state machine.
  uint8_t	Positive;	// specifies if it is positve or negative
  double	minTime;	// Minimum low frequency time in Ticks
  double	tGain;		// Low frequency Noise Gain
  double	overValue;	// Overshoot value
  uint32_t	overTime;	// Overshoot time in ticks
  double	underValue;	// Undershoot value
  uint32_t	underTime;	// Undershoot time in ticks
  uint32_t	tDiff;		// Time difference between last event and now in ticks
} SPid;

/*	Main PID Caluclation
	pid		-> Pointer to PID structure above
	value	-> Current output value, used for low frequency noise reduction (eg 0 to 1)
	Ticks	-> Ticks since CPU Startup (eg 1 tick per 10ms)
	error	-> Current error between desired position and read in position eg( position - input_read )
	position -> Target Position
*/
double UpdatePID(SPid * pid, double value, uint32_t* Ticks, double error, double position)
{
	double pTerm, dTerm, iTerm, tTerm;
	pTerm = pid->pGain * error;
	// calculate the proportional term
	// calculate the integral state with appropriate limiting
	pid->iState += error;

	if (pid->iState > pid->iMax)
		pid->iState = pid->iMax;
	else if (pid->iState < pid->iMin)
		pid->iState = pid->iMin;

	iTerm = pid->iGain * pid->iState;  // calculate the integral term
	dTerm = pid->dGain * (position - pid->dState);

	tTerm = 0; // Initialise the tTerm 
	if (pid->underValue > pid->overValue) // Perform a bounce check 
	{ 
		pid->overValue = pid->underValue; 
	} 
	if (error > 0) // Negative feedback required 
	{ 
		if (value < pid->underValue)
			pid->underValue = value;

		if (pid->dError <= 0) // Record Overshoot (value and time) 
		{ 
			pid->overValue = value; 
			pid->overTime = *Ticks; 
		} 

		pid->tDiff = *Ticks - pid->overTime;
		if (pid->tDiff > pid->minTime) // Perform the calculation 
		{ 
			tTerm = (pid->tDiff - pid->minTime) * pid->tGain * (pid->overValue - pid->underValue); 
		} 
	} 
	if (error < 0) // Positive feedback required 
	{ 
		if (value > pid->overValue)
			pid->overValue = value;

		if (pid->dError >= 0) // Record the Undershoot (value and time) 
		{ 
			pid->underValue = value; 
			pid->underTime = *Ticks; 
		} 

		pid->tDiff = *Ticks - pid->underTime;
		if (pid->tDiff > pid->minTime) // Perform the calculation 
		{ 
			tTerm = (pid->tDiff - pid->minTime) * pid->tGain * (pid->overValue - pid->underValue) * -1; 
		} 
	} 

	pid->dError = error;    // Save error for next time 
	pid->dState = position;
	// Calculate the time adjuster
	return pTerm + iTerm - dTerm + tTerm;
}